/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/

package com.nurdrobotics;

import edu.wpi.first.wpilibj.*;
import edu.wpi.first.wpilibj.can.CANTimeoutException;

/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the SimpleRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */
public class DriveTestBot extends SimpleRobot {
    // The following variables give names to each PWM ID for the Jaguar
    // connected to the corresponding motor.
    final int MOTORID_FRONT_LEFT = 1;
    final int MOTORID_REAR_LEFT = 4;
    final int MOTORID_FRONT_RIGHT = 3;
    final int MOTORID_REAR_RIGHT = 2;

    // The following variable give a name to each Joystick channel. This makes
    // it easier to know in the driver station which joystick should be matched
    // to which joystick port.
    final int JOYSTICKID_DRIVER = 1;

    // The following variables give names to joystick buttons
    final int BUTTONID_BOOST = 8;

    // The following variables give names to Driver Station analog sliders
    final int DSANALOG_BOOST_GAIN = 1;

    // The following variables define names for the Jaguar motor controllers
    Jaguar jaguarFrontLeft = null;
    Jaguar jaguarFrontRight = null;
    Jaguar jaguarRearLeft = null;
    Jaguar jaguarRearRight = null;

    // The following variables define names for the Joysticks
    Joystick joystickDriver; // joystick used by the driver

    // Define a variable to point to the single DriverStation object
    DriverStation ds = DriverStation.getInstance();

    public DriveTestBot() {
        // create a joystick to be used by the driver
        joystickDriver = new Joystick(JOYSTICKID_DRIVER);
    }

    /**
     * This function is called once each time the robot enters autonomous mode.
     */
    public void autonomous() {
        
    }

    /**
     * This function is called once each time the robot enters operator control.
     */
    public void operatorControl() {
        try {
            int motorID = 0;
            try {
                if(jaguarFrontLeft == null) {
                    motorID = MOTORID_FRONT_LEFT;
                    jaguarFrontLeft = new Jaguar(motorID);
                }
                if(jaguarFrontRight == null) {
                    motorID = MOTORID_FRONT_RIGHT;
                    jaguarFrontRight = new Jaguar(motorID);
                }
                if(jaguarRearLeft == null) {
                    motorID = MOTORID_REAR_LEFT;
                    jaguarRearLeft = new Jaguar(motorID);
                }
                if(jaguarRearRight == null) {
                    motorID = MOTORID_REAR_RIGHT;
                    jaguarRearRight = new Jaguar(motorID);
                }
            } catch (Exception ex) {
                System.out.println("Error initializing motorID " + motorID);
                ex.printStackTrace();
                System.out.println("Stack trace complete");
            }

            // create a robot drive system that consist of four motors
            //        drive = new RobotDrive(PWMChannelFrontLeftMotor, PWMChannelRearLeftMotor,
            //                PWMChannelFrontRightMotor, PWMChannelRearRightMotor);
            RobotDrive drive = new RobotDrive(jaguarFrontLeft, jaguarRearLeft, jaguarFrontRight, jaguarRearRight);

            // turn off motor safety
            drive.setSafetyEnabled(false);

            // declare variables for the stick inputs
            double moveValue;
            double rotateValue;

            // Boost factor is a scale from 0 to 1 to indicate the maximum
            // power when not in boost
            double powerFactor = 0.8;

            // enable the watchdog
            // the watchdog will automatically disable the robot if something
            // in the software hangs. As long as you feed the watchdog, it
            // will assume everything is ok. However, if you go too long
            // without feeding the watchdog, it will assume the software is
            // hung and disable the robot
            // set Expiration of Watchdog as 1.0 second
            getWatchdog().setExpiration(1.0);
            getWatchdog().setEnabled(true);

            // loop over the following instructions as long as the robot
            // is enabled and the mode is set to teleoperated (operator control)
            while(isEnabled() && isOperatorControl()) {
                // always feed the watchdog first to let it know everything is ok
                getWatchdog().feed();

                // get the move and rotate values from the joystick
                moveValue = -joystickDriver.getRawAxis(2);
                rotateValue = joystickDriver.getRawAxis(1);

                // if the boost button is not pressed, scale the move and rotate
                // values by the boost factor
                if (joystickDriver.getRawButton(BUTTONID_BOOST) == false) {
                    //Retrieve boost factor from the driver station analog dial
                    //Analog inputs reads 0 to 3.3
                    //Boost factor will be a scale from 0 to 1

                    //System.out.println("Joystick: " + moveValue + ", " + rotateValue + "PowerFactor" + powerFactor);

                    powerFactor = ds.getAnalogIn(DSANALOG_BOOST_GAIN) / 5.0;
                    
                    //Scale the moveValue and the rotateValue

                    moveValue = moveValue * powerFactor;
                    rotateValue = rotateValue * powerFactor;

                }

                // Drive the robot in arcarde mode using the move and rotate values
                drive.arcadeDrive(moveValue, rotateValue);

                System.out.println("move: " + moveValue +
                        " rotate = " + rotateValue +
                        ", frontLeft = " + jaguarFrontLeft.get() +
                        ", rearLeft = " + jaguarRearLeft.get() +
                        ", frontRight = " + jaguarFrontRight.get() +
                        ", rearRight = " + jaguarRearRight.get());

                // Sleep for 5 milliseconds to give the cRio a chance to
                // Process other events
                Timer.delay(.005);
            } // end of while loop
        } // end of try logic
        catch(Exception ex) {
            System.out.println("Unhandled exception in operatorControl, disabling");
            ex.printStackTrace();
        }
    }
}
